#include "pose_observe.h"

// predict - obs
void PoseObserve::ComputeResidualAndJacobianPoseGraph(const State& last_state, const Eigen::Vector3d &last_pose, const Eigen::Vector2d &last_velocity, const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residualXi, Eigen::Matrix<double, 5, 8> &jacobianXi, Eigen::Matrix<double, 5, 1> &residualXj, Eigen::Matrix<double, 5, 5> &jacobianXj, Eigen::Matrix<double, 5, 5> &jacobianXij, Eigen::Matrix<double, 5, 8> &jacobianXji) {
	PoseObserve::ComputeResidual(last_state, last_pose, last_velocity, residualXi);
	PoseObserve::ComputeResidual(state, pose, velocity, residualXj);

	// cout << "residualXi ================== : " << residualXi << endl;

	jacobianXi = Eigen::Matrix<double, 5, 8>::Zero();
	jacobianXi.block<5,5>(0,0) = Eigen::Matrix<double, 5, 5>::Identity();
	
    // cout << "jacobianXi == : \n" << jacobianXi << endl;

	jacobianXij = Eigen::Matrix<double, 5, 5>::Zero();
	jacobianXji = Eigen::Matrix<double, 5, 8>::Zero();

	jacobianXj = Eigen::Matrix<double, 5, 5>::Identity();
}


void PoseObserve::ComputeResidual(const State& state, const Eigen::Vector3d &pose, const Eigen::Vector2d &velocity, Eigen::Matrix<double, 5, 1> &residual) {
	Eigen::Vector2d res_p = state.p - pose.block<2,1>(0,0);
	// double res_theta = math_utils::normalize_angle(state.theta - pose[2]);
	double res_theta = math_utils::theta_normalize(SO2(state.R).log() - pose[2]);

	Eigen::Vector2d res_v = state.v - velocity;
	residual(0,0) = res_theta;
	residual.block<2,1>(1,0) = res_v;
	residual.block<2,1>(3,0) = res_p;
}
